/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2018 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_DUMMYMOTION_H_
#define __MMM_DUMMYMOTION_H_


#include <MMM/Motion/Motion.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>

namespace MMM
{

class DummyMotion : public Motion {
public:

    DummyMotion() : Motion("DummyMotion") {
        MMM_INFO << "Loading output model file for dummy motion." << std::endl;
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        std::string path = MMMTools_SRC_DIR;
        path += "/data/Model/Winter/mmm.xml";
        MMM::ModelPtr model = r->loadModel(path);
        if (!model) throw MMM::Exception::MMMException("Could not read model from " + path + "!");

        this->originalModel = model;
        this->processedModel = model;

        ModelPoseSensorPtr mpSensor(new ModelPoseSensor());
        Eigen::Vector3f root;
        root << 0.0f, 0.0f, 0.0f;
        ModelPoseSensorMeasurementPtr mpMeasurement(new ModelPoseSensorMeasurement(0.0f, root, root));
        mpSensor->addSensorMeasurement(mpMeasurement);
        this->addSensor(mpSensor);

        std::vector<std::string> jointNames = {"LSx_joint", "LSz_joint", "RSx_joint", "RSz_joint" };
        KinematicSensorPtr kSensor(new KinematicSensor(jointNames));
        Eigen::VectorXf jointAngles(4);
        jointAngles << 1.5f, 1.5f, 1.5f, -1.5f;
        KinematicSensorMeasurementPtr kMeasurement(new KinematicSensorMeasurement(0.0f, jointAngles));
        kSensor->addSensorMeasurement(kMeasurement);
        this->addSensor(kSensor);
    }
};

}

#endif
